/**
 * @file    filter.c
 * @brief   算法库-滤波算法
 * @author  Liuwei
 * @version 1.0.1
 * @date    2023-04-26
 * 
 * @copyright Copyright (c) 2023 JBD-Energy Storage Technology Co. LTD
 * 
 * @par 修改日志:
 * <table>
 * <tr><th>Date       <th>Version   <th>Author   <th>Description
 * <tr><td>2023-04-26 <td>1.0.1     <td>Liuwei   <td>首次创建
 * </table>
 */

/* includes ------------------------------------------------------------------*/
#include "filter.h"
#include "base_math.h"
#include "math.h"
/* macro/define --------------------------------------------------------------*/

/* typedef -------------------------------------------------------------------*/

/* local functions -----------------------------------------------------------*/

/* static variables ----------------------------------------------------------*/

/* global variables ----------------------------------------------------------*/

/** ------------------------------------------------------------------------- *
  *                             Global function
 ** ------------------------------------------------------------------------- */
/**
 * @brief  注册一个滤波器
 * @param  data_obj : 滤波对象
 * @param  data_buf : 数据缓冲区
 * @param  buf_size : 缓冲区大小
 * @param  value    : 跳变值
 */
void filter_login(filter_type *data_obj , uint16_t *data_buf , uint16_t buf_size , uint16_t value)
{
    data_obj->data_buf = data_buf;
    data_obj->buf_size = buf_size;
    data_obj->jump     = value;
    data_obj->now_idx  = 0;
    data_obj->calc_num = 0;
}

/**
 * @brief  添加一个数据到缓冲区
 * @param  data_obj : 数据对象
 * @param  data ： 数据
 */
uint16_t filter_calc(filter_type *data_obj , uint16_t data)
{
    int32_t data_diff = 0;
	int32_t last_data = 0;
	uint16_t i = 0;
	uint32_t data_sum = 0;
	uint16_t calc_val = 0;
	
	if(data_obj->calc_num == 0)
	{
		data_obj->data_buf[0] = data;
		data_obj->now_idx = 0;
		data_obj->calc_num = 1;

		return data;
	}

	last_data = data_obj->data_buf[data_obj->calc_num - 1];
    if(data >= last_data)
	    data_diff = data - last_data;
    else
        data_diff = last_data - data;

	if(data_diff > data_obj->jump)
	{
		data_obj->data_buf[0] = data;
		data_obj->now_idx = 0;
		data_obj->calc_num = 1;
		data_obj->jump_flag = 1;
		return data;
	}
	
	data_obj->jump_flag = 0;
	
	++ data_obj->now_idx;

	if(data_obj->now_idx >= data_obj->buf_size)
	{
		data_obj->now_idx = 0;
	}

	data_obj->data_buf[data_obj->now_idx] = data;

	++ data_obj->calc_num;

    data_obj->calc_num = CALC_MIN(data_obj->calc_num , data_obj->buf_size);

	for(i = 0;i < data_obj->calc_num; ++ i)
    {
        data_sum += data_obj->data_buf[i];
    }

	calc_val = 1.0 * data_sum / data_obj->calc_num + 0.5;

	return calc_val;
}

/**
 * @brief  添加一个数据到缓冲区
 * @param  data_obj : 数据对象
 * @param  data ： 数据
 */
uint16_t filter_deep_calc(filter_type *data_obj , uint16_t data)
{
    int32_t data_diff = 0;
	int32_t last_data = 0;
	uint16_t i = 0;
	uint32_t data_sum = 0;
	uint16_t calc_val = 0;
	
	if(data_obj->calc_num == 0)
	{
		data_obj->data_buf[0] = data;
		data_obj->now_idx = 0;
		data_obj->calc_num = 1;

		return data;
	}
	
	last_data = data_obj->data_buf[data_obj->calc_num - 1];
	
//    if(data >= last_data)
//	    data_diff = data - last_data;
//    else
//        data_diff = last_data - data;

//	if(data_diff > data_obj->jump)
//	{
//		data_obj->data_buf[0] = data;
//		data_obj->now_idx = 0;
//		data_obj->calc_num = 1;
//		data_obj->jump_flag = 1;
//		return data;
//	}
	
	data_obj->jump_flag = 0;
	
	++ data_obj->now_idx;

	if(data_obj->now_idx >= data_obj->buf_size)
	{
		data_obj->now_idx = 0;
	}

	data_obj->data_buf[data_obj->now_idx] = data;

	++ data_obj->calc_num;

    data_obj->calc_num = CALC_MIN(data_obj->calc_num , data_obj->buf_size);
	
	if(data_obj->calc_num < 6)
	{
		for(i = 0;i < data_obj->calc_num; ++ i)
		{
			data_sum += data_obj->data_buf[i];
		}

		calc_val = 1.0 * data_sum / data_obj->calc_num + 0.5;
	}
	else
	{
		for(i = 3;i < data_obj->calc_num - 3; ++ i)
		{
			data_sum += data_obj->data_buf[i];
		}
		
		calc_val = 1.0 * data_sum / (data_obj->calc_num - 6) + 0.5;
	}

	return calc_val;
}

void filter_lp_init(lp_filter_type *fir, int zero)
{
    double omega = 2 * PI*fir->type.fl*fir->ts;
    double  sine = sin(omega);
    double  cosine = cos(omega);
    double  alpha = sine / (2 * fir->q);

    fir->b[0] = fir->b[1] = fir->b[2] = fir->a[0] = fir->a[1] = fir->a[2] = 0;

    switch (fir->type.type)
    {
    case FILTER_1ST_LP:
    case FILTER_1ST_HP:
        fir->b[0] = (double)fir->type.fl * 2.0 * PI*fir->ts;
        fir->a[1] = 1 - fir->b[0];
        break;
    case FILTER_2ND_LP:
        fir->a[0] = 1 + alpha;
        fir->a[1] = (-2 * cosine) / fir->a[0];
        fir->a[2] = (1 - alpha) / fir->a[0];
        fir->b[0] = ((1 - cosine) / 2) / fir->a[0];
        fir->b[1] = (1 - cosine) / fir->a[0];
        fir->b[2] = ((1 - cosine) / 2) / fir->a[0];
        break;
    case FILTER_2ND_HP:
        fir->a[0] = 1 + alpha;
        fir->a[1] = (-2 * cosine) / fir->a[0];
        fir->a[2] = (1 - alpha) / fir->a[0];
        fir->b[0] = ((1 + cosine) / 2) / fir->a[0];
        fir->b[1] = (-(1 + cosine)) / fir->a[0];
        fir->b[2] = ((1 + cosine) / 2) / fir->a[0];
        break;
    case FILTER_2ND_BP:
    case FILTER_2ND_BS:
        fir->a[0] = 1 + alpha;
        fir->a[1] = (-2 * cosine) / fir->a[0];
        fir->a[2] = (1 - alpha) / fir->a[0];
        fir->b[0] = (sine / 2) / fir->a[0];
        fir->b[1] = 0;
        fir->b[2] = (-sine / 2) / fir->a[0];
        break;
    }
	
    if(zero)
        fir->x[0] = fir->x[1] = fir->x[2] = fir->y[0] = fir->y[1] = 0;
}

float filter_lp_calc(lp_filter_type *fir, float smp_in)
{
	float output_val = 0;
	float in_val = smp_in;
    if (fir->type.type == FILTER_1ST_LP || fir->type.type == FILTER_1ST_HP)
    {
        output_val = fir->b[0] * in_val + fir->a[1] * fir->y[0];
        fir->y[0] = output_val;
    }
    else if (fir->type.type)
    {
        fir->x[2] = fir->x[1];
        fir->x[1] = fir->x[0];
        fir->x[0] = in_val;
        output_val = fir->b[0] * fir->x[0] + fir->b[1] * fir->x[1] + fir->b[2] * fir->x[2] - fir->a[1] * fir->y[0] - fir->a[2] * fir->y[1];
        fir->y[1] = fir->y[0];
        fir->y[0] = output_val;
    }
        
    return output_val;
}

float filter_lp_calc_jump(lp_filter_type *fir, float smp_in)
{
	fir->x[0] = smp_in;
	fir->x[1] = smp_in;
	fir->x[2] = smp_in;
	fir->y[0] = smp_in;
	fir->y[1] = smp_in;
	
	return smp_in;
}

float filter_kalman_calc(kfp_coeff_type *kfp , float src)
{
	kfp->now = kfp->last + kfp->Q;
	
	kfp->gain = kfp->now / (kfp->now + kfp->R);
	
	kfp->out = kfp->out + kfp->gain * (src - kfp->out);
	
	kfp->last = (1 - kfp->gain) * kfp->now;
	
	return kfp->out;
}
 /** ------------------------------------------------------------------------- *
  *                              Local function
 ** ------------------------------------------------------------------------- */
 
/********** Copyright (c) 2023 JBD-Energy Storage Technology Co. LTD *********/


